
#include "ragdoll.h"
#include "graphics_functions.h"
#include "3d_primitives.h"
#include "3d_material.h"

using namespace WtfEngine;

#ifndef M_PI
#define M_PI       3.14159265358979323846
#endif

#ifndef M_PI_2
#define M_PI_2     1.57079632679489661923
#endif

#ifndef M_PI_4
#define M_PI_4     0.785398163397448309616
#endif

static btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
{
	bool isDynamic = (mass != 0.f);

	btVector3 localInertia(0,0,0);
	if (isDynamic)
		shape->calculateLocalInertia(mass,localInertia);

	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);

	PhysicsEngine::GetInstance().getWorld()->addRigidBody(body);

	return body;
}

RagDoll::RagDoll ( const RagDoll::tCreationOptions& opts )
: GameObject( opts ), m_ownerWorld ( PhysicsEngine::GetInstance().getWorld() )
{
	float scale = 10;

	// Setup the geometry
	mShapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(0.15 * scale), btScalar(0.20 * scale));
	mShapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(0.15 * scale), btScalar(0.28 * scale));
	mShapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(0.10 * scale), btScalar(0.05 * scale));
	mShapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07 * scale), btScalar(0.45 * scale));
	mShapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05 * scale), btScalar(0.37 * scale));
	mShapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07 * scale), btScalar(0.45 * scale));
	mShapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05 * scale), btScalar(0.37 * scale));
	mShapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05 * scale), btScalar(0.33 * scale));
	mShapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04 * scale), btScalar(0.25 * scale));
	mShapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05 * scale), btScalar(0.33 * scale));
	mShapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04 * scale), btScalar(0.25 * scale));

	// Setup the graphics
	WtfGraphics::Material::tRef rMat = new WtfGraphics::Material("", WtfGraphics::Material::tCreationOptions());

	mGfx[BODYPART_PELVIS] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(0.15 * scale, 0.20 * scale, 8));
	mGfx[BODYPART_SPINE] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(0.15 * scale, 0.28 * scale, 8));
	mGfx[BODYPART_HEAD] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(btScalar(0.10 * scale), btScalar(0.05 * scale), 8));
	mGfx[BODYPART_LEFT_UPPER_LEG] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(btScalar(0.07 * scale), btScalar(0.45 * scale), 8));
	mGfx[BODYPART_LEFT_LOWER_LEG] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(btScalar(0.05 * scale), btScalar(0.37 * scale), 8));
	mGfx[BODYPART_RIGHT_UPPER_LEG] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(btScalar(0.07 * scale), btScalar(0.45 * scale), 8));
	mGfx[BODYPART_RIGHT_LOWER_LEG] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(btScalar(0.05 * scale), btScalar(0.37 * scale), 8));
	mGfx[BODYPART_LEFT_UPPER_ARM] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(btScalar(0.05 * scale), btScalar(0.33 * scale), 8));
	mGfx[BODYPART_LEFT_LOWER_ARM] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(btScalar(0.04 * scale), btScalar(0.25 * scale), 8));
	mGfx[BODYPART_RIGHT_UPPER_ARM] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(btScalar(0.05 * scale), btScalar(0.33 * scale), 8));
	mGfx[BODYPART_RIGHT_LOWER_ARM] = new WtfGraphics::MaterialBinding("", rMat, new WtfGraphics::Primitives::Cylinder(btScalar(0.04 * scale), btScalar(0.25 * scale), 8));

	// Setup all the rigid bodies
	btTransform offset; offset.setIdentity();
	//offset.setOrigin(positionOffset);

	btTransform transform;
	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.  * scale), btScalar(1.  * scale), btScalar(0. * scale)));
	mBodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_PELVIS]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.), btScalar(1.2 * scale), btScalar(0.)));
	mBodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_SPINE]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.), btScalar(1.6 * scale), btScalar(0.)));
	mBodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_HEAD]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(-0.18 * scale), btScalar(0.65 * scale), btScalar(0.)));
	mBodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_LEFT_UPPER_LEG]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(-0.18 * scale), btScalar(0.2 * scale), btScalar(0.)));
	mBodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_LEFT_LOWER_LEG]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.18 * scale), btScalar(0.65 * scale), btScalar(0.)));
	mBodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_RIGHT_UPPER_LEG]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.2), btScalar(0.)));
	mBodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_RIGHT_LOWER_LEG]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(-0.35 * scale), btScalar(1.45 * scale), btScalar(0. * scale)));
	transform.getBasis().setEulerZYX(0,0,M_PI_2);
	mBodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_LEFT_UPPER_ARM]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(-0.7 * scale), btScalar(1.45 * scale), btScalar(0.)));
	transform.getBasis().setEulerZYX(0,0,M_PI_2);
	mBodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_LEFT_LOWER_ARM]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.35 * scale), btScalar(1.45 * scale), btScalar(0.)));
	transform.getBasis().setEulerZYX(0,0,-M_PI_2);
	mBodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_RIGHT_UPPER_ARM]);

	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(0.7 * scale), btScalar(1.45 * scale), btScalar(0.)));
	transform.getBasis().setEulerZYX(0,0,-M_PI_2);
	mBodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1. * scale), offset*transform, mShapes[BODYPART_RIGHT_LOWER_ARM]);

	// Setup some damping on the mBodies
	for (int i = 0; i < BODYPART_COUNT; ++i)
	{
		mBodies[i]->setDamping(0.05, 0.85);
		mBodies[i]->setDeactivationTime(0.8);
		mBodies[i]->setSleepingThresholds(1.6, 2.5);
	}

	// Now setup the constraints
	btHingeConstraint* hingeC;
	btConeTwistConstraint* coneC;

	btTransform localA, localB;

	localA.setIdentity(); localB.setIdentity();
	localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.15 * scale), btScalar(0.)));
	localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.15 * scale), btScalar(0.)));
	hingeC =  new btHingeConstraint(*mBodies[BODYPART_PELVIS], *mBodies[BODYPART_SPINE], localA, localB);
	hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2));
	mJoints[JOINT_PELVIS_SPINE] = hingeC;
	m_ownerWorld->addConstraint(mJoints[JOINT_PELVIS_SPINE], true);


	localA.setIdentity(); localB.setIdentity();
	localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30 * scale), btScalar(0.)));
	localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14 * scale), btScalar(0.)));
	coneC = new btConeTwistConstraint(*mBodies[BODYPART_SPINE], *mBodies[BODYPART_HEAD], localA, localB);
	coneC->setLimit(M_PI_4, M_PI_4, M_PI_2);
	mJoints[JOINT_SPINE_HEAD] = coneC;
	m_ownerWorld->addConstraint(mJoints[JOINT_SPINE_HEAD], true);


	localA.setIdentity(); localB.setIdentity();
	localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(btVector3(btScalar(-0.18 * scale), btScalar(-0.10 * scale), btScalar(0.)));
	localB.getBasis().setEulerZYX(0,0,-M_PI_4*5); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225 * scale), btScalar(0.)));
	coneC = new btConeTwistConstraint(*mBodies[BODYPART_PELVIS], *mBodies[BODYPART_LEFT_UPPER_LEG], localA, localB);
	coneC->setLimit(M_PI_4, M_PI_4, 0);
	mJoints[JOINT_LEFT_HIP] = coneC;
	m_ownerWorld->addConstraint(mJoints[JOINT_LEFT_HIP], true);

	localA.setIdentity(); localB.setIdentity();
	localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225 * scale), btScalar(0.)));
	localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185 * scale), btScalar(0.)));
	hingeC =  new btHingeConstraint(*mBodies[BODYPART_LEFT_UPPER_LEG], *mBodies[BODYPART_LEFT_LOWER_LEG], localA, localB);
	hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
	mJoints[JOINT_LEFT_KNEE] = hingeC;
	m_ownerWorld->addConstraint(mJoints[JOINT_LEFT_KNEE], true);


	localA.setIdentity(); localB.setIdentity();
	localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(btVector3(btScalar(0.18 * scale), btScalar(-0.10 * scale), btScalar(0.)));
	localB.getBasis().setEulerZYX(0,0,M_PI_4); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225 * scale), btScalar(0.)));
	coneC = new btConeTwistConstraint(*mBodies[BODYPART_PELVIS], *mBodies[BODYPART_RIGHT_UPPER_LEG], localA, localB);
	coneC->setLimit(M_PI_4, M_PI_4, 0);
	mJoints[JOINT_RIGHT_HIP] = coneC;
	m_ownerWorld->addConstraint(mJoints[JOINT_RIGHT_HIP], true);

	localA.setIdentity(); localB.setIdentity();
	localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225 * scale), btScalar(0.)));
	localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185 * scale), btScalar(0.)));
	hingeC =  new btHingeConstraint(*mBodies[BODYPART_RIGHT_UPPER_LEG], *mBodies[BODYPART_RIGHT_LOWER_LEG], localA, localB);
	hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
	mJoints[JOINT_RIGHT_KNEE] = hingeC;
	m_ownerWorld->addConstraint(mJoints[JOINT_RIGHT_KNEE], true);


	localA.setIdentity(); localB.setIdentity();
	localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(btVector3(btScalar(-0.2 * scale), btScalar(0.15 * scale), btScalar(0.)));
	localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18 * scale), btScalar(0.)));
	coneC = new btConeTwistConstraint(*mBodies[BODYPART_SPINE], *mBodies[BODYPART_LEFT_UPPER_ARM], localA, localB);
	coneC->setLimit(M_PI_2, M_PI_2, 0);
	mJoints[JOINT_LEFT_SHOULDER] = coneC;
	m_ownerWorld->addConstraint(mJoints[JOINT_LEFT_SHOULDER], true);

	localA.setIdentity(); localB.setIdentity();
	localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18 * scale), btScalar(0.)));
	localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14 * scale), btScalar(0.)));
	hingeC =  new btHingeConstraint(*mBodies[BODYPART_LEFT_UPPER_ARM], *mBodies[BODYPART_LEFT_LOWER_ARM], localA, localB);
	hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
	mJoints[JOINT_LEFT_ELBOW] = hingeC;
	m_ownerWorld->addConstraint(mJoints[JOINT_LEFT_ELBOW], true);



	localA.setIdentity(); localB.setIdentity();
	localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(btVector3(btScalar(0.2 * scale), btScalar(0.15 * scale), btScalar(0.)));
	localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18 * scale), btScalar(0.)));
	coneC = new btConeTwistConstraint(*mBodies[BODYPART_SPINE], *mBodies[BODYPART_RIGHT_UPPER_ARM], localA, localB);
	coneC->setLimit(M_PI_2, M_PI_2, 0);
	mJoints[JOINT_RIGHT_SHOULDER] = coneC;
	m_ownerWorld->addConstraint(mJoints[JOINT_RIGHT_SHOULDER], true);

	localA.setIdentity(); localB.setIdentity();
	localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18 * scale), btScalar(0.)));
	localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14 * scale), btScalar(0.)));
	hingeC =  new btHingeConstraint(*mBodies[BODYPART_RIGHT_UPPER_ARM], *mBodies[BODYPART_RIGHT_LOWER_ARM], localA, localB);
	hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
	mJoints[JOINT_RIGHT_ELBOW] = hingeC;
	m_ownerWorld->addConstraint(mJoints[JOINT_RIGHT_ELBOW], true);
}

RagDoll::~RagDoll ()
{
	int i;

	// Remove all constraints
	for ( i = 0; i < JOINT_COUNT; ++i)
	{
		m_ownerWorld->removeConstraint(mJoints[i]);
		delete mJoints[i]; mJoints[i] = 0;
	}

	// Remove all bodies and shapes
	for ( i = 0; i < BODYPART_COUNT; ++i)
	{
		m_ownerWorld->removeRigidBody(mBodies[i]);
		
		delete mBodies[i]->getMotionState();

		delete mBodies[i]; mBodies[i] = 0;
		delete mShapes[i]; mShapes[i] = 0;
	}
}

void RagDoll::Render()
{
	Transformation3D t;
	btScalar m[16];

	// Render all body parts
	for (int i = 0; i < BODYPART_COUNT; ++i)
	{
		mBodies[i]->getCenterOfMassTransform().getOpenGLMatrix(m);
		t = Transformation3D::FromOpenGLMatrix(m);
		
		WtfGraphics::SaveView();
		WtfGraphics::Transform(t);

		mGfx[i]->render();

		WtfGraphics::RestoreView();
	}
}


/**** test ****/
#include "input.h"
#include "render.h"

FUNCTOR(RagDollSpawn)
	RagDoll::tRef r = new RagDoll( RagDoll::tCreationOptions() );
	Renderer::GetInstance().PushObject( r );
ENDFUNC;

void RagDoll::InitTest()
{
	Kernel::GetInstance().GetGameState()->BindEvent( new MouseDownEvent(), new RagDollSpawn() );
}
